# motion planner
import numpy as np
import numpy
from algorithm.mathlib import utilities as utl
# def planninglinear(pose0, pose1, steps):
#     '''
#     :param pose0: init pose of trajectory
#     :param pose1: desired pose of trajectory
#     :param steps: interpolation points
#     :return: interpolation path set
#     '''
#     pose_traj = np.zeros((7, steps))
#     qtn0 = np.quaternion(pose0[3], pose0[4], pose0[5], pose0[6])
#     qtn1 = np.quaternion(pose1[3], pose1[4], pose1[5], pose1[6])
#     for i in range(0, steps):
#         qtn = utl.qtnSlerp(qtn0, qtn1, float(i+1)/float(steps))
#         pose_traj[0, i] = (pose0[0] * float(steps - i - 1) + pose1[1] * float(i+1))/float(steps)
#         pose_traj[1, i] = (pose0[1] * float(steps - i - 1) + pose1[2] * float(i+1))/float(steps)
#         pose_traj[2, i] = (pose0[2] * float(steps - i - 1) + pose1[3] * float(i+1))/float(steps)
#         pose_traj[3, i] = qtn.w
#         pose_traj[4, i] = qtn.x
#         pose_traj[5, i] = qtn.y
#         pose_traj[6, i] = qtn.z
#     return pose_traj

def planninglinear(pose0, pose1, t):
    pose = utl.PoseRobot()
    qtn = utl.qtnSlerp(pose0.qtn, pose1.qtn, t)
    pos = (1-t) * pose0.pos + t * pose1.pos
    pose.qtn = qtn
    pose.pos = pos
    return pose
